#人脸追踪
#import keyboard
import time
import board
import Board
import os
#open_io="sudo pigpiod"
#os.system(open_io)
import pigpio
#open_io="sudo motion"
#os.system(open_io)
time.sleep(1)
pi = pigpio.pi()
from queue import Queue

import myclient
coordinate_queue = Queue(1)


def set_servo_angle(channel, angle):  # 输入角度转换成12^精度的数值
    # print(angle)
    Board.setPWMServoAngle(channel,angle)

if __name__ == '__main__':
    myRaspConnection = myclient.connect_Raspberry('49.233.245.232', 8888)
    mag = "5_1_-1_-1#"
    myRaspConnection.send(mag)
    
    beangle_LR = 90   # 左右
    beangle_UD = 90  # 上下
    cen_y = 270  # 中心坐标
    cen_x = 440
    # 舵机插的通道口
    channel1 = 1  # 左右
    channel2 = 2  # 上下
    # 设置初始角度
    Board.setPWMServoAngle(channel1,beangle_LR)
    Board.setPWMServoAngle(channel2,beangle_UD) 
    # 变化幅度（这个越大，舵机动的幅度就越大）
    angleFreqLR = 1
    angleFreqUD = 1
    FX_L = 20
    FX_U = 10
    index = 0
    while True:
        msg = myRaspConnection.get()
        #print(msg)
        if msg != "":
            mess = msg.split('_')
            point = mess[1]
            x0 = int(point)>>16
            y0 = int(point) & 0xFFFF
            #print(x0,y0)
            #print("---")
            if y0 == 65535:
                mag = "0_5_-1_-1"
                myRaspConnection.send(mag)
                break
            # 超出屏幕外
            if x0 !=0 and y0 !=0 :
                #print(x0,y0)
                if x0 - cen_x >=FX_L :
                    beangle_LR -= angleFreqLR
                    # print(f"左：{beangle_LR}")
                    if beangle_LR <= 20:
                        beangle_LR = 20
                    set_servo_angle(channel1, beangle_LR)

                if cen_x - x0 >= FX_L:
                    beangle_LR += angleFreqLR
                    # print(f"右：{beangle_LR}")
                    if beangle_LR >= 160:
                        beangle_LR = 160

                    set_servo_angle(channel1, beangle_LR)

                if cen_y - y0 >= FX_U:
                    beangle_UD += angleFreqUD
                    # print(f"下：{beangle_UD}")
                    if beangle_UD >= 170:
                        beangle_UD = 170
                    set_servo_angle(channel2, beangle_UD)

                if y0-cen_y >= FX_U:
                    beangle_UD -= angleFreqUD
                    # print(f"上：{beangle_UD}")
                    if beangle_UD <= 80:
                        beangle_UD = 80
                    set_servo_angle(channel2, beangle_UD)
        if index == 10000:
            index=0
            myRaspConnection.send("5_1_-1_-1")
        index+=1

